////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNMemory/inc/WNMemory.h"
#include "WNCore/inc/WNAssert.h"
#include "WNMath/inc/WNVector3.h"
#include "WNMath/inc/WNBasicMath.h"
#include "WNMath/inc/WNExtendedMath.h"
#include "WNMath/inc/WNMatrix33.h"
#include "WNMath/inc/WNPoint2.h"
#include "WNMath/inc/WNPoint3.h"
#include "WNMath/inc/WNQuaternion.h"
#include "WNMath/inc/WNVector2.h"
#include "WNMath/inc/WNVector4.h"

namespace WNMath {
    WNVector3 operator + (WN_FLOAT32 _f, const WNVector3& _vector) {
        return(WNVector3(_f + _vector.x, _f + _vector.y, _f + _vector.z));
    }

    WNVector3 operator - (WN_FLOAT32 _f, const WNVector3& _vector) {
        return(WNVector3(_f - _vector.x, _f - _vector.y, _f - _vector.z));
    }

    WNVector3 operator * (WN_FLOAT32 _f, const WNVector3& _vector) {
        return(WNVector3(_f * _vector.x, _f * _vector.y, _f * _vector.z));
    }

    WNVector3 operator / (WN_FLOAT32 _f, const WNVector3& _vector) {
        const WN_FLOAT32 fInv = 1.0f / _f;

        return(WNVector3(fInv * _vector.x, fInv * _vector.y, fInv * _vector.z));
    }

    WNVector3 operator * (const WNQuaternion& _quaternion, const WNVector3& _vector) {
        return(_vector * _quaternion);
    }
}

using namespace WNMath;

// Constructors ////////////////////////////////////////////////////////////////////////////////////

WNVector3::WNVector3() :
    x(0.0f),
    y(0.0f),
    z(0.0f) {
}

WNVector3::WNVector3(WN_FLOAT32 _f) :
    x(_f),
    y(_f),
    z(_f) {
}

WNVector3::WNVector3(const WN_FLOAT32* _f) :
    x(_f[0]),
    y(_f[1]),
    z(_f[2]) {
}

WNVector3::WNVector3(const WNVector2& _vector, WN_FLOAT32 _f) :
    x(_vector.x),
    y(_vector.y),
    z(_f) {
}

WNVector3::WNVector3(WN_FLOAT32 _x, WN_FLOAT32 _y, WN_FLOAT32 _z) :
    x(_x),
    y(_y),
    z(_z) {
}

// Operators ///////////////////////////////////////////////////////////////////////////////////////

WNVector3::operator WN_FLOAT32* () {
    return(&x);
}

WNVector3::operator const WN_FLOAT32* () const {
    return(&x);
}

WN_FLOAT32& WNVector3::operator () (WN_INT32 _index) {
    WN_DEBUG_ASSERT(_index < 3);

    return(*(&x + _index));
}

WN_FLOAT32 WNVector3::operator () (WN_INT32 _index) const {
    WN_DEBUG_ASSERT(_index < 3);

    return(*(&x + _index));
}

WNVector3& WNVector3::operator += (const WNVector3& _vector) {
    x += _vector.x;
    y += _vector.y;
    z += _vector.z;

    return(*this);
}

WNVector3& WNVector3::operator -= (const WNVector3& _vector) {
    x -= _vector.x;
    y -= _vector.y;
    z -= _vector.z;

    return(*this);
}

WNVector3& WNVector3::operator *= (const WNQuaternion& _quaternion) {
    const WNQuaternion q = _quaternion;
    const WNVector3 vec = *this;
    const WN_FLOAT32 xw2 = q.x * q.w * 2.0f;
    const WN_FLOAT32 xy2 = q.x * q.y * 2.0f;
    const WN_FLOAT32 xz2 = q.x * q.z * 2.0f;
    const WN_FLOAT32 yw2 = q.y * q.w  *2.0f;
    const WN_FLOAT32 yz2 = q.y * q.z * 2.0f;
    const WN_FLOAT32 zw2 = q.z * q.w * 2.0f;
    const WN_FLOAT32 xxzz = q.x * q.x - q.z * q.z;
    const WN_FLOAT32 wwyy = q.w * q.w - q.y * q.y;

    x = (xxzz + wwyy) * vec.x + (xy2 + zw2) * vec.y + (xz2 - yw2) * vec.z;
    y = (xy2 - zw2) * vec.x + (q.y * q.y + q.w * q.w - q.x * q.x - q.z * q.z) * vec.y + (yz2 + xw2) * vec.z;
    z = (xz2 + yw2) * vec.x + (yz2 - xw2) * vec.y + (wwyy - xxzz) * vec.z;

    return(*this);
}

WNVector3& WNVector3::operator += (WN_FLOAT32 _f) {
    x += _f;
    y += _f;
    z += _f;

    return(*this);
}

WNVector3& WNVector3::operator -= (WN_FLOAT32 _f) {
    x -= _f;
    y -= _f;
    z -= _f;

    return(*this);
}

WNVector3& WNVector3::operator *= (WN_FLOAT32 _f) {
    x *= _f;
    y *= _f;
    z *= _f;

    return(*this);
}

WNVector3& WNVector3::operator *= (const WNVector3& _vector) {
    x *= _vector.x;
    y *= _vector.y;
    z *= _vector.z;

    return(*this);
}

WNVector3& WNVector3::operator /= (WN_FLOAT32 _f) {
    const WN_FLOAT32 fInv = 1.0f / _f;

    x *= fInv;
    y *= fInv;
    z *= fInv;

    return(*this);
}

WNVector3 WNVector3::operator + () const {
    return(*this);
}

WNVector3 WNVector3::operator - () const {
    return(WNVector3(-x, -y, -z));
}

WNVector3 WNVector3::operator + (const WNVector3& _vector) const {
    return(WNVector3(x + _vector.x, y + _vector.y, z + _vector.z));
}

WNVector3 WNVector3::operator - (const WNVector3& _vector) const {
    return(WNVector3(x - _vector.x, y - _vector.y, z - _vector.z));
}

WNVector3 WNVector3::operator * (const WNVector3& _vector) const{
    WNVector3 vector = *this;

    vector *= _vector;

    return(vector);
}

WNVector3 WNVector3::operator * (const WNQuaternion& _quaternion) const {
    WNVector3 vector = *this;

    vector *= _quaternion;

    return(vector);
}

WNVector3 WNVector3::operator + (WN_FLOAT32 _f) const {
    return(WNVector3(x + _f, y + _f, z + _f));
}

WNVector3 WNVector3::operator - (WN_FLOAT32 _f) const {
    return(WNVector3(x - _f, y - _f, z - _f));
}

WNVector3 WNVector3::operator * (WN_FLOAT32 _f) const {
    return(WNVector3(x * _f, y * _f, z * _f));
}

WNVector3 WNVector3::operator / (WN_FLOAT32 _f) const {
    const WN_FLOAT32 fInv = 1.0f / _f;

    return(WNVector3(x * fInv, y * fInv, z * fInv));
}

WN_BOOL WNVector3::operator == (const WNVector3& _vector) const {
    return(0 == WNMemCmp(this, &_vector, sizeof(WNVector3)));
}

WN_BOOL WNVector3::operator != (const WNVector3& _vector) const {
    return(0 != WNMemCmp(this, &_vector, sizeof(WNVector3)));
}

// Members /////////////////////////////////////////////////////////////////////////////////////////

WN_VOID WNVector3::Zero() {
    WNMemSet(&x, 0, sizeof(WNVector3));
}

WN_VOID WNVector3::Set(const WN_FLOAT32* _f) {
    WNMemCpy(&x, &_f[0], sizeof(WNVector3));
}

WN_VOID WNVector3::Set(WN_FLOAT32 _x, WN_FLOAT32 _y, WN_FLOAT32 _z) {
    x = _x;
    y = _y;
    z = _z;
}

WN_BOOL WNVector3::IsZero() const {
    return(x == 0.0f && y == 0.0f && z == 0.0f);
}

WN_VOID WNVector3::Normalize() {
    const WN_FLOAT32 invLength = WNInvSqrt(LengthSquared());

    *this *= invLength;
}

WN_VOID WNVector3::Truncate(WN_FLOAT32 _length) {
    if (_length != 0.0f) {
        const WN_FLOAT32 lengthSquared = LengthSquared();

        if (lengthSquared > (_length * _length)) {
            const WN_FLOAT32 invLength = _length * WNInvSqrt(lengthSquared);

            *this *= invLength;
        }
    } else {
        Zero();
    }
}

WN_VOID WNVector3::Clamp(const WNVector3& _min, const WNVector3& _max) {
    const WNVector3 vec = *this;
    const WNVector3 vecMin = _min;
    const WNVector3 vecMax = _max;

    if (vec.x > vecMax.x) {
        x = vecMax.x;
    } else if (vec.x < vecMin.x) {
        x = vecMin.x;
    }

    if (vec.y > vecMax.y) {
        y = vecMax.y;
    } else if (vec.y < vecMin.y) {
        y = vecMin.y;
    }

    if (vec.z > vecMax.z) {
        z = vecMax.z;
    } else if (vec.z < vecMin.z) {
        z = vecMin.z;
    }
}

WN_VOID WNVector3::Snap(WNSnapDirection _direction) {
    const WNVector3 vec = *this;

    switch(_direction) {
    case WN_SNAP_NEAREST:
        x = WNRound(vec.x);
        y = WNRound(vec.y);
        z = WNRound(vec.z);

        break;
    case WN_SNAP_UP:
        x = WNCeil(vec.x);
        y = WNCeil(vec.y);
        z = WNCeil(vec.z);

        break;
    case WN_SNAP_DOWN:
        x = WNFloor(vec.x);
        y = WNFloor(vec.y);
        z = WNFloor(vec.z);

        break;
    case WN_SNAP_TRUNCATE:
        x = WNTrunc(vec.x);
        y = WNTrunc(vec.y);
        z = WNTrunc(vec.z);
    }
}

WN_VOID WNVector3::Reflect(const WNVector3& _normal) {
    const WNVector3 vec = *this;

    *this = (vec - (2.0f * _normal * WNDot(vec, _normal)));
}

WN_VOID WNVector3::Rotate(const WNQuaternion& _quaternion) {
    const WNVector3 vec = *this;

    *this = vec * _quaternion;
}

WN_VOID WNVector3::Scale(WN_FLOAT32 _amount) {
    x *= _amount;
    y *= _amount;
    z *= _amount;
}

WN_VOID WNVector3::Scale(WN_FLOAT32 _x, WN_FLOAT32 _y, WN_FLOAT32 _z) {
    x *= _x;
    y *= _y;
    z *= _z;
}

WN_VOID WNVector3::Scale(const WNVector3& _scale) {
    x *= _scale.x;
    y *= _scale.y;
    z *= _scale.z;
}

WN_FLOAT32 WNVector3::Length() const {
    const WNVector3 vec = *this;

    return(WNSqrt((vec.x * vec.x) + (vec.y * vec.y) + (vec.z * vec.z)));
}

WN_FLOAT32 WNVector3::LengthSquared() const {
    const WNVector3 vec = *this;

    return((vec.x * vec.x) + (vec.y * vec.y) + (vec.z * vec.z));
}

WNVector3 WNVector3::GetNormalized() const {
    WNVector3 vec = *this;

    return(vec.Normalize(), vec);
}

WNVector3 WNVector3::GetTruncated(WN_FLOAT32 _length) const {
    WNVector3 vec = *this;

    return(vec.Truncate(_length), vec);
}

WNVector3 WNVector3::GetClamped(const WNVector3& _min, const WNVector3& _max) const {
    WNVector3 vec = *this;

    return(vec.Clamp(_min, _max), vec);
}

WNVector3 WNVector3::GetSnaped(WNSnapDirection _direction) const {
    WNVector3 vec = *this;

    return(vec.Snap(_direction), vec);
}

WNVector3 WNVector3::GetReflected(const WNVector3& _normal) const {
    WNVector3 vec = *this;

    return(vec.Reflect(_normal), vec);
}

WNVector3 WNVector3::GetRotated(const WNQuaternion& _quaternion) const {
    WNVector3 vec = *this;

    return(vec.Rotate(_quaternion), vec);
}

WNVector3 WNVector3::GetScaled(WN_FLOAT32 _scale) const {
    WNVector3 vec = *this;

    return(vec.Scale(_scale), vec);
}

WNPoint2 WNVector3::ToPoint2() const {
    return(WNPoint2(x, y));
}

WNPoint3 WNVector3::ToPoint3() const {
    return(WNPoint3(x, y, z));
}

WNVector2 WNVector3::ToVector2() const {
    return(WNVector2(x, y));
}

WNVector4 WNVector3::ToVector4() const {
    return(WNVector4(x, y, z, 1.0f));
}